cmake_minimum_required(VERSION 3.5)
project(hybrid_a_star_debug)

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

# SET(CMAKE_CXX_FLAGS "-O0 -g ${CMAKE_CXX_FLAGS}") # 关闭优化编译，方便调试
SET(CMAKE_CXX_FLAGS "-O2 -g ${CMAKE_CXX_FLAGS}")
set(CMAKE_BUILD_TYPE Debug)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
  set(CMAKE_BUILD_TYPE Release)
endif()

# explicitly set policies we already support in newer cmake versions
if(POLICY CMP0074)
  # TODO: update *_ROOT variables to be PCL_*_ROOT or equivalent.
  # CMP0074 directly affects how Find* modules work and *_ROOT variables.  Since
  # this is a config file that will be consumed by parent projects with (likely)
  # NEW behavior, we need to push a policy stack.
  cmake_policy(SET CMP0074 NEW)
endif()

find_package(Eigen3)
if(NOT EIGEN3_FOUND)
  # Fallback to cmake_modules
  find_package(cmake_modules REQUIRED)
  find_package(Eigen REQUIRED)
  set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
else()
  set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2 REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(yaml_cpp_vendor REQUIRED)
find_package(ompl REQUIRED)
find_package(yaml-cpp REQUIRED)

include_directories(include
    ${EIGEN3_INCLUDE_DIRS}
    ${OMPL_INCLUDE_DIRS}
    ${YAML_CPP_INCLUDE_DIR}
)

add_executable(hybrid_a_star_debug
    src/replan_node.cpp
    src/replanning.cpp
    src/hybrid_a_star.cpp
)
ament_target_dependencies(hybrid_a_star_debug rclcpp std_msgs geometry_msgs nav_msgs tf2_ros tf2 visualization_msgs
tf2_geometry_msgs yaml_cpp_vendor)
target_link_libraries(hybrid_a_star_debug ${OMPL_LIBRARIES})

install(TARGETS
  hybrid_a_star_debug 
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
  DESTINATION lib/${PROJECT_NAME}
)

install(
  DIRECTORY launch param map
  DESTINATION share/${PROJECT_NAME}/
)
ament_export_include_directories(include)
ament_package()
